#include <iostream>
#include "HRLocomotion/hr_locomotion.h"
#include "Eigen/Dense"
#include "qpOASES.hpp"

using std::cout;
using std::endl;
int main(){
    std::cout << "Starting MPC Constraints test." << std::endl;

    /** MPC parameters */
    int m_steps = 2;
    int n_periods_step = 8;
    int N_horizon = m_steps*n_periods_step;
    hrLocomotion::real_t T_sampling = 0.1;
    hrLocomotion::real_t com_height = 0.3 ;

       /** These parameters describe the state of the robot */

    hrLocomotion::Vector3r footState;    //Member for the position of the current foot on the ground
    hrLocomotion::Vector3r nextFootState;    //Member for the position of the current foot on the ground
    hrLocomotion::RobotState robotState;        //Member for the augmented CoM robot state
    hrLocomotion::Vector3r xk;              //Variable for the CoM robot state in X
    hrLocomotion::Vector3r yk;              //Variable for the CoM robot state in Y
    hrLocomotion::Vector3r thetak;          //Variable for the CoM robot state in Theta
    hrLocomotion::VectorXr x_vel(N_horizon);        //Reference velocity in X direction     //TODO: Time list trajectories similar to ALDCM
    hrLocomotion::VectorXr y_vel(N_horizon);        //Reference velocity in Y direction     //TODO
    hrLocomotion::FootStatus footStatus = hrLocomotion::kLeftFootOnGround;
    hrLocomotion::Vector3r jerkXYTheta;

    hrLocomotion::VectorXr uk;      ///Jerk vector
    uk.resize(2*(N_horizon+m_steps));
    uk.setZero();
    /** Initialization of the robot parameters */

    cout << "Init robot paramters" << endl;
    footState << 0.0,0.05,0.0;
    xk << 0.0 , 0.0, 0.0;
    yk << 0.05 , 0.0, 0.0;
    thetak << 0.0, 0.0 ,0.0;
    robotState.setState(xk,yk,thetak);

    jerkXYTheta << 0.0,0.,0.;

    x_vel.setOnes();
    y_vel.setOnes();
    x_vel = x_vel*1.00;
    y_vel = y_vel*0.00;


    /** Initialization of the MPC */
    hrLocomotion::MPCData mpcData = hrLocomotion::MPCData(com_height,T_sampling,m_steps,n_periods_step);
    hrLocomotion::MPCConstraints constraints;

    hrLocomotion::RobotSystem system;
    system.init(com_height,T_sampling);
    mpcData.init();     //0

    cout << "Hessian" << endl << mpcData.getHessian().trace() << endl;                                       //TODO: Review version with different parameters for m and steps
    cout << "F vector" << endl << mpcData.getFvector(robotState,footState,x_vel,y_vel) << endl;     //TODO: Review version with different parameters for m and steps

    cout << "Calling constraints init method" << endl;
    constraints.init(mpcData);
    cout << "Called init method" << endl;
    constraints.setConstraints(mpcData,footStatus, footState, robotState);

    cout << "Constraints Hessian" << endl << constraints.getAMatrix().trace() << endl;                                       //TODO: Review version with different parameters for m and steps
    cout << "Constraints Bounds" << endl << constraints.getbupVector() << endl;     //TODO: Review version with different parameters for m and steps


    ///This part of the code should not be here, but this is a simple test

    int nWSR = 1000;
    int nV = 2*(N_horizon+m_steps);
    int nC = constraints.getbupVector().rows();
    boost::shared_ptr<qpOASES::SQProblem> quadprog;
    qpOASES::Options options;
    options.setToMPC();
    //options.printLevel = qpOASES::PL_NONE;
    quadprog.reset(new qpOASES::SQProblem(nV,nC));
    quadprog->setOptions(options);
    quadprog->init(mpcData.getHessian().data(), mpcData.getFvector(robotState,footState,x_vel,y_vel).data(),constraints.getAMatrix().data(),NULL,NULL,NULL,constraints.getbupVector().data(),nWSR);
    if(quadprog->getPrimalSolution(uk.data()) == qpOASES::RET_QP_NOT_SOLVED){
        return hrLocomotion::FAILED_INIT;
        cout << "failed to solve" << endl;
    }
        cout << "qpoases solved" << endl;

        jerkXYTheta(0) = uk(0);
        jerkXYTheta(1) = uk(N_horizon+m_steps);
        nextFootState(0) = uk(N_horizon);
        nextFootState(1) = uk(N_horizon+m_steps+N_horizon);


    cout << "Uk size:" << endl << uk.rows() << endl;
    cout << "Uk:" << endl << uk << endl;

    //

    cout << "Everything seems to be working fine..." << endl;
    cout << "Before Robot State" << endl << robotState << endl;
    system.nextState(jerkXYTheta,robotState);
    cout <<  "Applied Jerk" << endl << jerkXYTheta << endl;
    cout << "New Robot State" << endl << robotState << endl;
    cout << "Current foot:" << endl << footState << endl;
    cout << "Next foot:" << endl << nextFootState << endl;

}



